#include "network.h"

struct rt_ringbuffer rb;
uint8_t rb_pool[512];

static int sock, connected, bytes_received;
socklen_t sin_size;
static struct sockaddr_in server_addr, client_addr;

volatile rt_bool_t isConnected;
uint8_t send_buf[256] = "This is TCP Server from RT-Thread.";
uint16_t send_len;

uint8_t rcv_cmd[32];
uint8_t rcv_len;


void server_init()
{
    isConnected = 0;
    rt_ringbuffer_init(&rb, rb_pool, 512);
    if ((sock = socket(AF_INET, SOCK_STREAM, 0)) == -1)
    {
        /* 创建失败的错误处理 */
        rt_kprintf("Socket error\n");
        return;
    }
    int32_t nonblock = 1;

    /* 初始化服务端地址 */
    server_addr.sin_family = AF_INET;
    server_addr.sin_port = htons(5000); /* 服务端工作的端口 */
    server_addr.sin_addr.s_addr = INADDR_ANY;
    rt_memset(&(server_addr.sin_zero), 0, sizeof(server_addr.sin_zero));

    if (bind(sock, (struct sockaddr *)&server_addr, sizeof(struct sockaddr)) == -1)
    {
        /* 绑定失败 */
        rt_kprintf("Unable to bind\n");
        return;
    }

    if (listen(sock, 5) == -1)
    {
        rt_kprintf("Listen error\n");
        return;
    }
    rt_kprintf("\nTCPServer Waiting for client on port 5000...\n");
    server_handle();
}

void server_handle()
{
    int32_t ret;
    rt_thread_t tRx, tTx, tCmd;
    tRx = rt_thread_create("TCP RX", func_recv, RT_NULL, 2048, 17, 150);
    tCmd = rt_thread_create("RX CMD", rx_cmd, RT_NULL, 2048, 20, 150);

    rt_thread_startup(tRx);
    rt_thread_startup(tCmd);
    while (1)
    {
        sin_size = sizeof(struct sockaddr_in);
        connected = accept(sock, (struct sockaddr *)&client_addr, &sin_size);

        // 连接失败
        if (connected < 0)
        {
            rt_kprintf("accept connection failed! errno = %d\n", errno);
            continue;
        }
        rt_kprintf("I got a connection from (%s , %d)\n",
                   inet_ntoa(client_addr.sin_addr), ntohs(client_addr.sin_port));
        isConnected = 1;
        // 至此，与客户端建立起了链接。以下是在该链接上进行的处理

        ret = send(connected, send_buf, 34, 0);
        if (ret < 0)
        {
            /* 发送失败，关闭这个连接 */
            closesocket(connected);
            isConnected = 0;
            rt_kprintf("\nsend error,close the socket.\r\n");
            break;
        }
        if (ret == 0)
            rt_kprintf("\n Send warning,send function return 0.\r\n");

        while (isConnected)
            ;
    }
}

void func_recv()
{
    int32_t ret, bytes_received;
    uint8_t buf[256];
    rt_memset(buf, 0, 256);
    while (1)
    {
        if (isConnected == 0)
        {
            rt_thread_mdelay(100);
            continue;
        }

        bytes_received = recv(connected, buf, 256, 0);
        if (bytes_received < 0)
        {

            closesocket(connected);
            isConnected = 0;
            continue;
        }
        else if (bytes_received == 0)
        {
            rt_kprintf("\nReceived warning,recv function return 0.\r\n");
            closesocket(connected);
            isConnected = 0;
            continue;
        }
        rt_ringbuffer_put(&rb, buf, bytes_received);
        rt_memset(buf, 0, 256);
    }
}

void rx_cmd()
{
    uint8_t buf;
    while (1)
    {
        while (RT_RINGBUFFER_EMPTY == rt_ringbuffer_data_len(&rb))
            rt_thread_mdelay(10);
        rt_ringbuffer_get(&rb, rcv_cmd + rcv_len, 1);
        rcv_len++;

        if (rcv_cmd[rcv_len - 1] == '\n')
        {
            rcv_cmd[rcv_len] = 0;

            // 读取位置信息
            struct vector3 dest;
            if (strstr(rcv_cmd, "X") != NULL && strstr(rcv_cmd, "Y") != NULL)
            {
                sscanf(rcv_cmd, "X%fY%f", &dest.x, &dest.y);
                motion_goto(&motion, &dest);
                rt_kprintf("the position of head is set to %.2f,%.2f\r\n", dest.x, dest.y);
            }

            rcv_len = 0;
            rt_memset(rcv_cmd, 0, 32);
        }
    }
}

MSH_CMD_EXPORT(server_init, init the server);